/**
 * Copyright (c) The openTCS Authors.
 * <p>
 * This program is free software and subject to the MIT license. (For details,
 * see the licensing information (LICENSE.txt) you should have received with
 * this copy of the software.)
 */
package org.opentcs.strategies.basic.dispatching;

import com.google.common.collect.Lists;
import jakarta.inject.Inject;

import java.util.*;

import static java.util.Objects.requireNonNull;

import java.util.function.Predicate;
import java.util.stream.Collectors;

import lombok.AllArgsConstructor;
import lombok.Data;
import org.opentcs.components.kernel.Router;
import org.opentcs.components.kernel.services.InternalTransportOrderService;
import org.opentcs.data.TCSObjectReference;
import org.opentcs.data.model.*;
import org.opentcs.data.order.DriveOrder;
import org.opentcs.data.order.ReroutingType;
import org.opentcs.data.order.Route;
import org.opentcs.data.order.Route.Step;
import org.opentcs.data.order.TransportOrder;
import org.opentcs.drivers.vehicle.VehicleController;
import org.opentcs.drivers.vehicle.VehicleControllerPool;
import org.opentcs.strategies.basic.dispatching.DefaultDispatcherConfiguration.ReroutingImpossibleStrategy;

import static org.opentcs.strategies.basic.dispatching.DefaultDispatcherConfiguration.ReroutingImpossibleStrategy.IGNORE_PATH_LOCKS;
import static org.opentcs.strategies.basic.dispatching.DefaultDispatcherConfiguration.ReroutingImpossibleStrategy.PAUSE_AT_PATH_LOCK;
import static org.opentcs.strategies.basic.dispatching.DefaultDispatcherConfiguration.ReroutingImpossibleStrategy.PAUSE_IMMEDIATELY;

import org.opentcs.strategies.basic.dispatching.rerouting.DyReroutingStrategy;
import org.opentcs.strategies.basic.dispatching.rerouting.ReroutingStrategy;
import org.opentcs.strategies.basic.dispatching.rerouting.VehiclePositionResolver;
import org.opentcs.strategies.basic.scheduling.ReservationPool;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

/**
 * Provides some utility methods used for rerouting vehicles.
 */
public class RerouteUtil {

  /**
   * This class's logger.
   */
  private static final Logger LOG = LoggerFactory.getLogger(RerouteUtil.class);
  /**
   * The router.
   */
  private final Router router;
  /**
   * The vehicle controller pool.
   */
  private final VehicleControllerPool vehicleControllerPool;
  /**
   * The object service.
   */
  private final InternalTransportOrderService transportOrderService;

  private final DefaultDispatcherConfiguration configuration;

  private final Map<ReroutingType, ReroutingStrategy> reroutingStrategies;

  private final VehiclePositionResolver vehiclePositionResolver;

  private final ReservationPool reservationPool;
  private final DyReroutingStrategy dyReroutingStrategy;

  private Map<String, RerouteMsg> reRouteVehicleMsg = new HashMap<>();

  /**
   * Creates a new instance.
   *
   * @param router                  The router.
   * @param vehicleControllerPool   The vehicle controller pool.
   * @param transportOrderService   The object service.
   * @param configuration           The configuration.
   * @param reroutingStrategies     The rerouting strategies to select from.
   * @param vehiclePositionResolver Used to resolve the position of vehicles.
   */
  @Inject
  public RerouteUtil(Router router, VehicleControllerPool vehicleControllerPool, InternalTransportOrderService transportOrderService, DefaultDispatcherConfiguration configuration, Map<ReroutingType, ReroutingStrategy> reroutingStrategies, VehiclePositionResolver vehiclePositionResolver, ReservationPool reservationPool, DyReroutingStrategy dyReroutingStrategy) {
    this.router = requireNonNull(router, "router");
    this.vehicleControllerPool = requireNonNull(vehicleControllerPool, "vehicleControllerPool");
    this.transportOrderService = requireNonNull(transportOrderService, "transportOrderService");
    this.configuration = requireNonNull(configuration, "configuration");
    this.reroutingStrategies = requireNonNull(reroutingStrategies, "reroutingStrategies");
    this.vehiclePositionResolver = requireNonNull(vehiclePositionResolver, "vehiclePositionResolver");
    this.reservationPool = requireNonNull(reservationPool, "reservationPool");
    this.dyReroutingStrategy = requireNonNull(dyReroutingStrategy, "dyReroutingStrategy");
  }

  public void reroute(Collection<Vehicle> vehicles, ReroutingType reroutingType) {
    for (Vehicle vehicle : vehicles) {
      reroute(vehicle, reroutingType);
    }
  }

  public void reroute(Vehicle vehicle, ReroutingType reroutingType) {
    requireNonNull(vehicle, "vehicle");
    LOG.debug("Trying to reroute vehicle '{}'...", vehicle.getName());

    if (!vehicle.isProcessingOrder()) {
      LOG.debug("{} can't be rerouted without processing a transport order.", vehicle.getName());
      return;
    }

    TransportOrder originalOrder = transportOrderService.fetchObject(TransportOrder.class, vehicle.getTransportOrder());

    Optional<List<DriveOrder>> optOrders;
    if (reroutingStrategies.containsKey(reroutingType)) {
      optOrders = reroutingStrategies.get(reroutingType).reroute(vehicle);
    } else {
      LOG.warn("Cannot reroute {} for unknown rerouting type: {}", vehicle.getName(), reroutingType.name());
      optOrders = Optional.empty();
    }

    // Get the drive order with the new route or stick to the old one
    List<DriveOrder> newDriveOrders;
    if (optOrders.isPresent()) {
      newDriveOrders = optOrders.get();
    } else {
      newDriveOrders = updatePathLocksAndRestrictions(vehicle, originalOrder);
    }

    LOG.debug("Updating transport order {}...", originalOrder.getName());
    updateTransportOrder(originalOrder, newDriveOrders, vehicle);
  }

  public void dyReroute() {
    //todo:这里使用动态的dj算法进行四次重路由，选择可重路由并且花费最少的重路由进行操作
    Set<String> names = reservationPool.getBlockVehicleNames();
    Vehicle targetVehicle = null;
    List<DriveOrder> newDriveOrders = null;
    List<RerouteResult> rerouteResults = new ArrayList<>();
    for (String name : names) {
      Vehicle vehicle = transportOrderService.fetchObject(Vehicle.class, name);
      if (!vehicle.isProcessingOrder()) continue;
      Optional<List<DriveOrder>> reroute = dyReroutingStrategy.reroute(vehicle);
      if (reroute.isEmpty()) {
        LOG.info("{} 路径出现了无限大的路径，不使用", name);
        continue;
      }
      List<DriveOrder> driveOrders = reroute.get();
      Step step = driveOrders.get(0).getRoute().getSteps().get(0);
      //过滤掉新路径和老路径一样的情况
      if (reservationPool.getBlockSourceByVehicleName(name).contains(step.getPath().getName())) {
        LOG.info("{}重路由路径与老路径相同，不使用", name);
        continue;
      }
      rerouteResults.add(new RerouteResult(vehicle, driveOrders));
    }
    Optional<RerouteResult> bestDriverOrders = findBestDriverOrders(rerouteResults);
    if (bestDriverOrders.isPresent()) {
      RerouteResult rerouteResult = bestDriverOrders.get();
      targetVehicle = rerouteResult.getVehicle();
      newDriveOrders = rerouteResult.getOrders();
    }

    if (newDriveOrders == null) {
      LOG.warn("进行重路由，但是无法重路由有效小车");
      reservationPool.cleanRingBLocks();
      return;
    }
    TransportOrder originalOrder = transportOrderService.fetchObject(TransportOrder.class, targetVehicle.getTransportOrder());
    //合并新老订单
    Optional<List<DriveOrder>> driveOrders = dyReroutingStrategy.getDriveOrders(targetVehicle, newDriveOrders, originalOrder);
    //清除环记录
//    reservationPool.removeBlockMessage(targetVehicle.getName());
    Step step = newDriveOrders.get(0).getRoute().getSteps().get(0);
    String currentName = step.getSourcePoint() == null ? null : step.getSourcePoint().getName();
    //如果是重复路由，路由次数加1
    RerouteMsg orDefault = reRouteVehicleMsg.getOrDefault(targetVehicle.getName(), new RerouteMsg("", -1, "", 0));
    RerouteMsg newMsg = orDefault.getOrderName().equals(originalOrder.getName()) ? new RerouteMsg(originalOrder.getName(), originalOrder.getCurrentRouteStepIndex(), currentName, orDefault.getRerouteTime() + 1) : new RerouteMsg(originalOrder.getName(), originalOrder.getCurrentRouteStepIndex(), currentName, 1);
    reRouteVehicleMsg.put(targetVehicle.getName(), newMsg);
    LOG.info("new driverOrders: {}，进行重路由的步骤有{}", driveOrders.get().get(0), Arrays.toString(driveOrders.get().get(0).getRoute().getSteps().stream().filter(var -> var.getReroutingType() != null).toArray()));
    LOG.info("old driverOrders: {},进行重路由的步骤有{},执行到index为{}", originalOrder.getCurrentDriveOrder(), Arrays.toString(originalOrder.getCurrentDriveOrder().getRoute().getSteps().stream().filter(var -> var.getReroutingType() != null).toArray()), originalOrder.getCurrentRouteStepIndex());
    LOG.info("{} 进行动态重路由", targetVehicle.getName());
    updateTransportOrder(originalOrder, driveOrders.get(), targetVehicle);

  }

  /**
   * 找出环中重路由结果最好的小车
   *
   * @param rerouteResults
   * @return
   */
  private Optional<RerouteResult> findBestDriverOrders(List<RerouteResult> rerouteResults) {
    Optional<RerouteResult> first = rerouteResults.stream().sorted().filter((resultVar -> {
      TCSObjectReference<TransportOrder> transportOrder = resultVar.getVehicle().getTransportOrder();
      TransportOrder transportOrder1 = transportOrderService.fetchObject(TransportOrder.class, transportOrder);
      return !isSameRoute(resultVar.vehicle.getName(), transportOrder1);
    })).findFirst();
    if (first.isEmpty()) {
      //选择最少路由的进行路由
      first = rerouteResults.stream().min(Comparator.comparingLong(var->reRouteVehicleMsg.get(var.getVehicle().getName()).getRerouteTime()*var.getOrders().get(0).getRoute().getCosts()));
    }
    return first;
  }

  /**
   * 判断是否是重复路由，如果不是会清理之前的老重路由记录
   * @param vehicleName
   * @param order
   * @return
   */
  private boolean isSameRoute(String vehicleName, TransportOrder order) {
    RerouteMsg rerouteMsg = reRouteVehicleMsg.get(vehicleName);
    if (rerouteMsg == null) return false;
    int currentRouteStepIndex = order.getCurrentRouteStepIndex();
    if (!rerouteMsg.getOrderName().equals(order.getName()) || rerouteMsg.getStepIndex() != currentRouteStepIndex) {
      //如果不相等，那么就清除旧的。
      reRouteVehicleMsg.remove(vehicleName);
      return false;
    } else {
      return true;
    }
  }

  private List<DriveOrder> updatePathLocksAndRestrictions(Vehicle vehicle, TransportOrder originalOrder) {
    LOG.debug("Couldn't find a new route for {}. Updating the current one...", vehicle.getName());
    // Get all unfinished drive order of the transport order the vehicle is processing
    List<DriveOrder> unfinishedOrders = new ArrayList<>();
    unfinishedOrders.add(originalOrder.getCurrentDriveOrder());
    unfinishedOrders.addAll(originalOrder.getFutureDriveOrders());

    unfinishedOrders = updatePathLocks(unfinishedOrders);
    unfinishedOrders = markRestrictedSteps(unfinishedOrders, new ExecutionTest(configuration.reroutingImpossibleStrategy(), vehiclePositionResolver.getFutureOrCurrentPosition(vehicle)));
    return unfinishedOrders;
  }

  private void updateTransportOrder(TransportOrder originalOrder, List<DriveOrder> newDriveOrders, Vehicle vehicle) {
    VehicleController controller = vehicleControllerPool.getVehicleController(vehicle.getName());

    // Restore the transport order's history
    List<DriveOrder> newOrders = new ArrayList<>();
    newOrders.addAll(originalOrder.getPastDriveOrders());
    newOrders.addAll(newDriveOrders);

    // Update the transport order's drive orders with the re-routed ones
    LOG.debug("{}: Updating drive orders with {}.", originalOrder.getName(), newOrders);
    transportOrderService.updateTransportOrderDriveOrders(originalOrder.getReference(), newOrders);

    // If the vehicle is currently processing a (drive) order (and not waiting to get the next
    // drive order) we need to update the vehicle's current drive order with the new one.
    if (vehicle.hasProcState(Vehicle.ProcState.PROCESSING_ORDER)) {
      controller.setTransportOrder(transportOrderService.fetchObject(TransportOrder.class, originalOrder.getReference()));
    }

    // Let the router know the vehicle selected another route
    router.selectRoute(vehicle, newOrders);
  }

  private List<DriveOrder> updatePathLocks(List<DriveOrder> orders) {
    List<DriveOrder> updatedOrders = new ArrayList<>();

    for (DriveOrder order : orders) {
      List<Step> updatedSteps = new ArrayList<>();

      for (Step step : order.getRoute().getSteps()) {
        Path path = transportOrderService.fetchObject(Path.class, step.getPath().getReference());
        updatedSteps.add(new Route.Step(path, step.getSourcePoint(), step.getDestinationPoint(), step.getVehicleOrientation(), step.getRouteIndex()));
      }

      Route updatedRoute = new Route(updatedSteps, order.getRoute().getCosts());

      DriveOrder updatedOrder = new DriveOrder(order.getDestination()).withRoute(updatedRoute).withState(order.getState()).withTransportOrder(order.getTransportOrder());
      updatedOrders.add(updatedOrder);
    }

    return updatedOrders;
  }

  private List<DriveOrder> markRestrictedSteps(List<DriveOrder> orders, Predicate<Step> executionTest) {
    if (configuration.reroutingImpossibleStrategy() == IGNORE_PATH_LOCKS) {
      return orders;
    }
    if (!containsLockedPath(orders)) {
      return orders;
    }

    List<DriveOrder> updatedOrders = new ArrayList<>();
    for (DriveOrder order : orders) {
      List<Step> updatedSteps = new ArrayList<>();

      for (Step step : order.getRoute().getSteps()) {
        boolean executionAllowed = executionTest.test(step);
        LOG.debug("Marking path '{}' allowed: {}", step.getPath(), executionAllowed);
        updatedSteps.add(new Step(step.getPath(), step.getSourcePoint(), step.getDestinationPoint(), step.getVehicleOrientation(), step.getRouteIndex(), executionAllowed));
      }

      Route updatedRoute = new Route(updatedSteps, order.getRoute().getCosts());

      DriveOrder updatedOrder = new DriveOrder(order.getDestination()).withRoute(updatedRoute).withState(order.getState()).withTransportOrder(order.getTransportOrder());
      updatedOrders.add(updatedOrder);
    }

    return updatedOrders;
  }

  private boolean containsLockedPath(List<DriveOrder> orders) {
    return orders.stream().map(order -> order.getRoute().getSteps()).flatMap(steps -> steps.stream()).anyMatch(step -> step.getPath().isLocked());
  }


  private class ExecutionTest implements Predicate<Step> {

    /**
     * The current fallback strategy.
     */
    private final ReroutingImpossibleStrategy strategy;
    /**
     * The (earliest) point from which execution may not be allowed.
     */
    private final Point source;
    /**
     * Whether execution of a step is allowed.
     */
    private boolean executionAllowed = true;

    /**
     * Creates a new intance.
     *
     * @param strategy The current fallback strategy.
     * @param source   The (earliest) point from which execution may not be allowed.
     */
    ExecutionTest(ReroutingImpossibleStrategy strategy, Point source) {
      this.strategy = requireNonNull(strategy, "strategy");
      this.source = requireNonNull(source, "source");
    }

    @Override
    public boolean test(Step step) {
      if (!executionAllowed) {
        return false;
      }

      switch (strategy) {
        case PAUSE_IMMEDIATELY:
          if (Objects.equals(step.getSourcePoint(), source)) {
            executionAllowed = false;
          }
          break;
        case PAUSE_AT_PATH_LOCK:
          if (step.getPath().isLocked()) {
            executionAllowed = false;
          }
          break;
        default:
          executionAllowed = true;
      }

      return executionAllowed;
    }
  }

  @Data
  @AllArgsConstructor
  private class RerouteMsg {
    private String orderName;
    //最后完成的一个step的index
    private int stepIndex;
    private String rerouteCurrentPointName;
    private long rerouteTime;

    @Override
    public boolean equals(Object obj) {
      if (!(obj instanceof RerouteMsg)) {
        return false;
      }
      RerouteMsg other = (RerouteMsg) obj;
      return this.orderName.equals(other.orderName) && this.stepIndex == other.stepIndex && (other.getRerouteCurrentPointName() == null || this.getRerouteCurrentPointName() == null || this.rerouteCurrentPointName.equals(other.rerouteCurrentPointName));
    }
  }

  @Data
  @AllArgsConstructor
  private class RerouteResult implements Comparable<RerouteResult> {
    private Vehicle vehicle;
    private List<DriveOrder> orders;
//    private int rerouteDriverIndex;

    @Override
    public int compareTo(RerouteResult o) {

      return (int) (this.getOrders().get(0).getRoute().getCosts() - o.getOrders().get(0).getRoute().getCosts());
    }
  }

}
